#include <sstream>
#include <cassert>
#include <set>
#include <list>
#include <iostream>
#include <algorithm>


// component headers
#include <tree_UPGMA.h>
#include <matrix_fill_lacunae.h>
#define DEBUG_LEVEL 5
#include <debug_func.h>

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>

using std::vector;
using std::list;
using std::string;
//using std::cerr;
using std::copy;
using std::cout;
using std::ostringstream;
using bcpp_tree::t_tree;
using bcpp_tree::t_node;
using namespace bcpp_matrix;

namespace upgma_tree
{


//----------------------------------------------------------------------------------------

//	upgma_make_tree

//----------------------------------------------------------------------------------------
struct t_value_pos
{
	double		value;
	unsigned	pos1;
	unsigned	pos2;
	t_value_pos(double v, unsigned p1, unsigned p2)
		: value(v), pos1(p1), pos2(p2){}
};
bool operator < (const t_value_pos& a, const t_value_pos& b)
{
	return (a.value < b.value);
}

using std::multiset;

//
// hybrid of list and vector:
// can refer to index by [position]
// can get list of available indices
//
class t_list_indices
{
public:
	typedef std::list<unsigned>::iterator iterator;
	typedef std::list<unsigned>::const_iterator const_iterator;
private:
	std::list<unsigned>			indices;
	std::vector<iterator>		indices_p;
public:
	iterator					begin() {return indices.begin();}
	iterator					end()   {return indices.end();}
	const_iterator				begin() const {return indices.begin();}
	const_iterator				end()  const  {return indices.end();}
	unsigned					size() const {return indices_p.size();}
	t_list_indices(unsigned sz)
	{
		for (unsigned i = 0; i < sz; ++i)
			indices_p.push_back(
				indices.insert(indices.end(), i));
	}
	void unset(unsigned pos)
	{
		assert(pos < size());
		if (indices_p[pos] != end())
		{
			indices.erase(indices_p[pos]);
			indices_p[pos] = end();
		}
	}
	void set(unsigned pos)
	{
		assert(pos < size());
		if (indices_p[pos] == end())
		{
			indices_p[pos] =
						indices.insert(indices.end(), pos);
		}
	}

};

using boost::numeric::ublas::compressed_matrix;
using boost::numeric::ublas::mapped_matrix;

void upgma_make_tree(	bcpp_tree::t_tree& tree,
						const t_sparse_matrix_f& orig_matrix,
						bool do_fill_lacunae,
						t_progress_indicator& dots)
{
	debug_func_cerr(5);

	assert(orig_matrix.names.size() == orig_matrix.size());
	unsigned cnt_leaves = orig_matrix.size();

#ifndef NDEBUG
	// make sure matrix is symmetrical!!
	for (unsigned i = 1; i < cnt_leaves; ++i)
		for (unsigned j = 0; j < i; ++j)
			assert(orig_matrix.matrix(i, j) == orig_matrix.matrix(i, j));
	for (unsigned i = 0; i < cnt_leaves - 1 ; ++i)
		for (unsigned j = i + 1; j < cnt_leaves; ++j)
			assert(orig_matrix.matrix(i, j) == orig_matrix.matrix(i, j));
#endif



	// make copy of matrix data so we don't clobber it
	t_sparse_matrix_f new_matrix;
	if (do_fill_lacunae)
	{
		new_matrix = orig_matrix;
		fill_lacunae(new_matrix);
	}
	const t_sparse_matrix_f& working_matrix(do_fill_lacunae ? new_matrix: orig_matrix);


	//
	// make 1. sorted set of values which know their matrix indices
	//      2. matrix of iterators into sorted set
	// i.e. can go from minimum value <-> matrix position quickly
	//
	debug_cerr(5, "insert into set" );
	typedef list<t_value_pos> t_sorted_values ;
	typedef t_sorted_values::iterator t_set_iter;
	t_sorted_values sorted_values;

	mapped_matrix<t_set_iter> matrix_lookup;
	matrix_lookup.resize(cnt_leaves, cnt_leaves, false);

	
    // assign matrix of iterators into linked list of values
	typedef t_sparse_matrix_f::sparse_matrix_type::const_iterator1 t_i1;
	typedef t_sparse_matrix_f::sparse_matrix_type::const_iterator2 t_i2;
	for (t_i1 i1 = working_matrix.matrix.begin1(), 
		 end1 = working_matrix.matrix.end1(); i1 != end1; ++i1)
	{
		for (t_i2 i2 = i1.begin(); i2 != i1.end(); ++i2)
		{
			unsigned ii = i2.index1();
			unsigned jj = i2.index2();
			if (ii >= jj)
				continue;

			///std::cerr << ii << "," << jj << "=" << *i2 << "\n";
			sorted_values.push_front(t_value_pos(*i2, ii, jj));
			matrix_lookup(ii, jj) =
			matrix_lookup(jj, ii) = sorted_values.begin();
		}
	}
	debug_cerr(5, "insert into set finished." );


	// prepare a list of clusters each of which starts out with one leaf
	vector<t_node*> cluster;
	for (unsigned i = 0; i < working_matrix.names.size(); ++i)
		cluster.push_back(&tree.add_leaf(working_matrix.names[i], i));


	//
	//	make a list of available clusters and pointers to their nodes
	//
	t_list_indices available_clusters(cnt_leaves);


	//dots.print_indicator('!');

	vector<double>		avg			(cnt_leaves, 0.0);
	vector<unsigned>	cluster_sz	(cnt_leaves, 1U);


	unsigned per_cluster_inc = cluster.size() * 3 / 2;
	// iterate (cluster - 1) times
	unsigned cnt_cluster = cluster.size();
	while (cnt_cluster > 1)
	{
		dots += cnt_cluster + per_cluster_inc;
		//
		// Lookup minimum distance in matrix using set
		//
		t_set_iter p_min = std::min_element(sorted_values.begin(), sorted_values.end());
		assert(p_min != sorted_values.end());
		unsigned	min1 = p_min->pos1;
		unsigned	min2 = p_min->pos2;

		double half_dist = p_min->value * 0.5;
		debug_cerr(5, "cluster #" << cnt_cluster << " between " << min1 << " and " << min2);

		//
		// merge cluster min2 into cluster min1
		//
		debug_cerr(5, "set length = " << (half_dist - avg[min1]));
		cluster[min1]->set_length(half_dist - avg[min1]);
		cluster[min2]->set_length(half_dist - avg[min2]);

		if (--cnt_cluster == 1)
			break;

		cluster[min1] = &tree.join_nodes(*cluster[min1], *cluster[min2]);
		cluster[min2] = 0;

		//
		// remove both cluster min1 and min2 from available clusters
		// cluster min1 will be readded at the next step
		//
		available_clusters.unset(min1);
		available_clusters.unset(min2);


		//
		// use cluster min2 distances to re-weight cluster min1 distances in matrix
		//
		double cluster_sz1 = cluster_sz[min1];
		double cluster_sz2 = cluster_sz[min2];
		double inv_cluster_sz = 1.0 / (cluster_sz1 + cluster_sz2);
		for (t_list_indices::iterator i = available_clusters.begin(),
			 end = available_clusters.end(); i != end; ++i)
		{
			unsigned pos = *i;
			debug_cerr(5, "i = " << pos << ",min1 = " << min1  << ",min2 = " << min2);

			t_set_iter *p1 = matrix_lookup.find_element(pos, min1); 
			t_set_iter *p2 = matrix_lookup.find_element(pos, min2);
			
			 
			double new_value;

			// adjust if both distances are less than 5000
			if (p1 && p2)
				new_value = ((*p1)->value * cluster_sz1 + 
							 (*p2)->value * cluster_sz2 ) * inv_cluster_sz;

			// transfer other sorted_values over, should really reweight cluster_sz somehow
			else if (p2)
			{
				new_value = (*p2)->value;
			}

			// else keep original value in *p1!
			else
				continue;

			if (p1)
				(*p1)->value = new_value;
			else
			{
				sorted_values.push_front(t_value_pos(new_value, pos,  min1));
				matrix_lookup(pos, min1) =
				matrix_lookup(min1, pos) = sorted_values.begin();
			}
		}

		//
		// cluster min2 distances are discarded from matrix
		//
		//		but add back min1 first to available clusters
		//		(matrix[min1][min2] should also be discarded)
		available_clusters.set(min1);
		for (t_list_indices::iterator i = available_clusters.begin(),
			 end = available_clusters.end(); i != end; ++i)
		{
			unsigned pos = *i;
			//assert(matrix_lookup(pos,min2) != sorted_values.end());
			debug_cerr(5, "erase: pos = " << pos << ",min1 = " << min1  << ",min2 = " << min2);
			t_set_iter *p1 = matrix_lookup.find_element(pos,min2);
			if (p1)
			{
				sorted_values.erase(*p1);
				//matrix_lookup.erase_element(pos,min2);
				//matrix_lookup.erase_element(min2, pos);
			}
			
		}

		// update cluster min1 size with cluster min2
		cluster_sz[min1] += cluster_sz[min2];
		cluster_sz[min2] = 0;
		avg[min1] = half_dist;
	}

	// last standing cluster still attached to root
	tree.reparent_to_grandparent();
	tree.finish();
	return;
}

//________________________________________________________________________________________

//		upgma_make_tree

//			wrapper

//________________________________________________________________________________________
void upgma_make_tree(	bcpp_tree::t_tree& cur_tree,
						const bcpp_matrix::t_sparse_matrix_f& matrix,
						bool fill_lacunae)
{
	ostringstream ignore_strm;
	t_progress_indicator dots(ignore_strm, 10000);
	upgma_make_tree(cur_tree, matrix, fill_lacunae, dots);
}




}

